/*
 * File:          my_controller-Luntui.c
 * Date:
 * Description:
 * Author:
 * Modifications:
 */

/*
 * You may need to add include files like <webots/distance_sensor.h> or
 * <webots/motor.h>, etc.
 */
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/position_sensor.h>
#include <stdio.h>
#include <webots/inertial_unit.h>
#include <webots/gyro.h>
#include <math.h>
#include "my_controller_Luntui.h"

#include "update.h"
#include "angular.h"

// #include "INS_task.h"
/*
 * You may want to add macros here.
 */

/*
 * This is the main program.
 * The arguments of the main function can be specified by the
 * "controllerArgs" field of the Robot node
 */

vmc_leg_t vmc;
chassis_t chassis_move;

void wheel_init()
{
  vmc.right_l1 = 0.06507f; // 第一杆的长度，单位m
  vmc.left_l1 = 0.06507f;  // 第一杆的长度，单位m
  chassis_move.phi_set = 0.28f;
  chassis_move.leg_set = 0.09f;
}
float a[4];
int main(int argc, char **argv)
{
  /* necessary to initialize webots stuff */
  wb_robot_init();

  /*
   * You should declare here WbDeviceTag variables for storing
   * robot devices like this:
   *  WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
   *  WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
   */
  wheel_init();
  /*4电机*/
  WbDeviceTag left_motor = wb_robot_get_device("L_motor");      // 左腿电机
  WbDeviceTag right_motor = wb_robot_get_device("R_motor");     // 右腿电机
  WbDeviceTag L_LUN_motor = wb_robot_get_device("L_LUN_motor"); // 左轮电机
  WbDeviceTag R_LUN_motor = wb_robot_get_device("R_LUN_motor"); // 右轮电机
  /*4电机编码器*/
  WbDeviceTag R_LUN_ENCODER = wb_robot_get_device("encoder_R_LUN"); // 右轮电机编码器
  WbDeviceTag L_LUN_ENCODER = wb_robot_get_device("encoder_L_LUN"); // 左轮电机编码器
  WbDeviceTag L_MOTOR = wb_robot_get_device("encoder_L_MOTOR");     // 左腿电机编码器
  WbDeviceTag R_MOTOR = wb_robot_get_device("encoder_R_MOTOR");     // 右腿电机编码器

  WbDeviceTag gyro = wb_robot_get_device("my_gyro");         // 陀螺仪
  WbDeviceTag inertial = wb_robot_get_device("my_inertial"); // IMU

  double leg_pos_taget = 0.5f; // 0.5 -1.5
  // wb_motor_set_position(left_motor, leg_pos_taget - 0.8f);
  // wb_motor_set_position(right_motor, -leg_pos_taget + 0.8f);

  wb_motor_set_position(left_motor, INFINITY);
  wb_motor_set_velocity(left_motor, 0.0);

  wb_motor_set_position(right_motor, INFINITY);
  wb_motor_set_velocity(right_motor, 0.0);

  wb_motor_set_position(L_LUN_motor, INFINITY);
  wb_motor_set_velocity(L_LUN_motor, 0.0);

  wb_motor_set_position(R_LUN_motor, INFINITY);
  wb_motor_set_velocity(R_LUN_motor, 0.0);

  wb_position_sensor_enable(R_LUN_ENCODER, TIME_STEP);
  wb_position_sensor_enable(L_LUN_ENCODER, TIME_STEP);
  wb_position_sensor_enable(L_MOTOR, TIME_STEP);
  wb_position_sensor_enable(R_MOTOR, TIME_STEP);

  wb_gyro_enable(gyro, TIME_STEP);
  wb_inertial_unit_enable(inertial, TIME_STEP);
  /* main loop
   * Perform simulation steps of TIME_STEP milliseconds
   * and leave the loop when the simulation is over
   */
  while (wb_robot_step(TIME_STEP) != -1)
  {

    const double *angular_velocity = wb_gyro_get_values(gyro);          // rad/s
    const double *data = wb_inertial_unit_get_roll_pitch_yaw(inertial); // rad

    double Rposision = wb_position_sensor_get_value(R_LUN_ENCODER); // 右轮编码器 rad
    double Lposision = wb_position_sensor_get_value(L_LUN_ENCODER); // 左轮编码器 rad

    double LTUIposision = wb_position_sensor_get_value(L_MOTOR) + 0.8f; // 左腿编码器 rad
    double RTUIposision = wb_position_sensor_get_value(R_MOTOR) - 0.8f; // 右腿编码器 rad// 实机顺时针为正

    // printf("L: %.3f -- R: %.3f  \n", chassis_move.joint_motor[0].pos, chassis_move.joint_motor[1].pos);

    // const double *data = wb_inertial_unit_get_roll_pitch_yaw(inertial);
    // 输出欧拉角（单位：弧度）
    // printf("Roll: %.3f rad, Pitch: %.3f rad, Yaw: %.3f rad\n",
    // data[0], data[1], data[2]);

    calculate_angular_velocity(&chassis_move, Rposision, Lposision, RTUIposision, LTUIposision);
    chassisR_feedback_update(&chassis_move, &vmc, angular_velocity, data); // 更新数据
    // printf("L: %.3f -- R: %.3f  \n", chassis_move.joint_motor[0].pos, chassis_move.joint_motor[1].pos);
    // printf("pos: %.3f \n", chassis_move.joint_motor[0].pos);
    // printf("right_len: %.3f -- left_len: %.3f \n", vmc.right_len, vmc.left_len);
    for (int i = 0; i < 4; i++)
    {
      lqr_k[i] = LQR_K_calc(&Poly_Coefficient[i][0], vmc.left_len);
    }
    for (int j = 4; j < 8; j++)
    {
      lqr_k[j] = LQR_K_calc(&Poly_Coefficient[j][0], vmc.right_len);
    }
    // printf("myPithR: %.3f \n", chassis_move.myPithR);

    chassis_move.phi_set = -3.81f * ((vmc.left_len + vmc.right_len) / 2.0f) + 0.31f; // 腿长变化导致机械中值发生变化
    // printf("phi_sety: %.3f -- x: %.3f\n", chassis_move.phi_set, ((vmc.left_len + vmc.right_len) / 2.0f));
    // chassis_move.phi_set = 0;
    chassis_move.wheel_motor[0].wheel_T = lqr_k[0] * (chassis_move.x_set - chassis_move.x) + lqr_k[1] * (chassis_move.v_set - chassis_move.v) + lqr_k[2] * (chassis_move.phi_set - chassis_move.myPithR) + lqr_k[3] * (chassis_move.d_phi_set - chassis_move.myPithGyroR);
    chassis_move.wheel_motor[0].wheel_T = -chassis_move.wheel_motor[0].wheel_T;
    chassis_move.wheel_motor[1].wheel_T = lqr_k[4] * (chassis_move.x_set - chassis_move.x) + lqr_k[5] * (chassis_move.v_set - chassis_move.v) + lqr_k[6] * (chassis_move.phi_set - chassis_move.myPithR) + lqr_k[7] * (chassis_move.d_phi_set - chassis_move.myPithGyroR);

    // a[0] = lqr_k[0] * (chassis_move.x_set - chassis_move.x);
    // // a[0] = chassis_move.x_set;
    // a[1] = lqr_k[1] * (0.7f * chassis_move.v_set - chassis_move.v);
    // a[2] = lqr_k[2] * (chassis_move.phi_set - chassis_move.myPithR);
    // a[3] = lqr_k[3] * (chassis_move.d_phi_set - chassis_move.myPithGyroR);
    // printf("a0: %.3f--a1: %.3f--a2: %.3f--a3: %.3f \n", a[0], a[1], a[2], a[3]);

    // printf("PithGyroR: %.3f \n", (chassis_move.d_phi_set - chassis_move.myPithGyroR));
    // printf("v: %.3f \n", chassis_move.v);
    // printf("x: %.3f \n", (chassis_move.x_set - chassis_move.x));
    // printf("v_lun: %.3f \n", chassis_move.wheel_motor[1].vel);
    // chassis_move.wheel_motor[0].wheel_T = chassis_move.wheel_motor[0].wheel_T + turn_T;
    // chassis_move.wheel_motor[1].wheel_T = chassis_move.wheel_motor[1].wheel_T - turn_T;
    // chassis_move.wheel_motor[0].wheel_T=0.0f-chassis_move.wheel_motor[0].wheel_T;

    vmc.left_F0 = mg + leg_kp * (chassis_move.leg_set - vmc.left_len) + leg_kd * vmc.left_len_dot; // 沿杆方向的力，单位：牛N
    vmc.right_F0 = mg + leg_kp * (chassis_move.leg_set - vmc.right_len) + leg_kd * vmc.right_len_dot;
    // vmc.left_F0 = mg; // 沿杆方向的力，单位：牛N
    // vmc.right_F0 = mg;

    // printf("left_len_dot: %.3f --right_len_dot: %.3f --left_len: %.3f --right_len: %.3f --pos: %.3f--pos: %.3f\n", vmc.left_len_dot, vmc.right_len_dot, vmc.left_len, vmc.right_len, chassis_move.joint_motor[0].pos, chassis_move.joint_motor[1].pos);
    // vmc.left_F0 = mg + leg_kp * (chassis_move.leg_set - vmc.left_len) + leg_kd * vmc.left_len_dot;
    // vmc.right_F0 = mg + leg_kp * (chassis_move.leg_set - vmc.right_len) + leg_kd * vmc.right_len_dot;
    // printf("left_F0: %.3f \n", vmc.left_F0);
    // printf("right_F0: %.3f \n", vmc.right_F0);

    // vmc.left_T1 = -2.0f * vmc.left_l1 * sin(chassis_move.joint_motor[0].pos) * vmc.left_F0;
    // // vmc.left_T1 = -vmc.left_T1;
    // vmc.right_T1 = -2.0f * vmc.right_l1 * sin(chassis_move.joint_motor[1].pos) * vmc.right_F0;

    // 关节电机 --沿杆方向的力转化为电机转矩Nm
    vmc.roll_F0 = roll_kp * (chassis_move.roll_set - chassis_move.myRoll) + roll_kd * chassis_move.myRollGyroR;
    printf("err: %.3f -- myRollGyroR: %.3f \n", (chassis_move.roll_set - chassis_move.myRoll), chassis_move.myRollGyroR);
    // vmc.roll_F0 = -vmc.roll_F0;
    vmc.left_F0 = vmc.left_F0 + vmc.roll_F0; // 叠加roll轴上的力
    vmc.right_F0 = vmc.right_F0 - vmc.roll_F0;

    // printf("myRollGyroR: %.3f \n", chassis_move.myRollGyroR);
    vmc.left_T1 = -2.0f * vmc.left_l1 * sin(chassis_move.joint_motor[0].pos) * vmc.left_F0; // 沿杆方向的力转化为电机转矩Nm
    vmc.right_T1 = 2.0f * vmc.right_l1 * sin(chassis_move.joint_motor[1].pos) * vmc.right_F0;
    vmc.left_T1 = 0.0f - vmc.left_T1;
    // printf("left_T1: %.3f \n", vmc.left_T1);
    // printf("right_T1: %.3f \n", vmc.right_T1);
    // float max = 0.18f;

    //////////

    mySaturate(&chassis_move.wheel_motor[0].wheel_T, -0.18f, 0.18f); // 轮子的输出力矩
    mySaturate(&chassis_move.wheel_motor[1].wheel_T, -0.18f, 0.18f);

    mySaturate(&vmc.left_T1, -1.6f, 1.6f);
    mySaturate(&vmc.right_T1, -1.6f, 1.6f);
    ////////////
    // printf("T: %.3f \n", chassis_move.wheel_motor[0].wheel_T);

    // wb_motor_set_position(left_motor, 0);
    // wb_motor_set_position(right_motor, 0);

    wb_motor_set_torque(left_motor, -vmc.left_T1);
    wb_motor_set_torque(right_motor, -vmc.right_T1);

    // wb_motor_set_velocity(L_LUN_motor, chassis_move.wheel_motor[0].wheel_T);
    // wb_motor_set_velocity(R_LUN_motor, chassis_move.wheel_motor[1].wheel_T);
    // wb_motor_set_velocity(L_LUN_motor, 3);
    // wb_motor_set_velocity(R_LUN_motor, 3);
    // printf("rads: %.3f ,%.3f \n", chassis_move.wheel_motor[0].vel, chassis_move.wheel_motor[1].vel);

    wb_motor_set_torque(L_LUN_motor, (1 * chassis_move.wheel_motor[0].wheel_T));
    wb_motor_set_torque(R_LUN_motor, (1 * chassis_move.wheel_motor[1].wheel_T));
    // wb_motor_set_torque(L_LUN_motor, 0.085);
    // wb_motor_set_torque(R_LUN_motor, 0.085);
  };

  /* Enter your cleanup code here */

  /* This is necessary to cleanup webots resources */
  wb_robot_cleanup();

  return 0;
}
